#include <SoftwareSerial.h>
#include <CyberLib.h>
//#define motors_init {D4_Out; D5_Out; D6_Out; D7_Out;} 
//#define robot_go {D4_Low; D5_High; D6_High; D7_Low;} 
//#define robot_back {D4_High; D5_Low; D6_Low; D7_High;}
//#define robot_stop {D4_Low; D5_Low; D6_Low; D7_Low;} 
//#define robot_rotation_left {D4_Low; D5_High; D6_Low; D7_High;} 
//#define robot_rotation_right {D4_High; D5_Low; D6_High; D7_Low;}
uint8_t inByte;

SoftwareSerial btSerial(9,10);
char incomingByte;  // входящие данные
int  LED = 13;      // LED подключен к 13 пину
int leftDirPin = 4;
int leftSpeedPin = 5;
  
int rightDirPin = 7;
int rightSpeedPin = 6;
  
//подбираем приемлемую скорость  
int runSpeed = 225;
int runSpeed1 = 50;
//динамик
int din=11;
//уз сонар
int sonarTrig=14;  
int sonarEhcho=15;  

#define size_buff 5 //размер массива sensor
uint16_t sensor[size_buff]; //массив для хранения замеров дальномера
uint8_t stat=0; //направление разворота

void setup()
{
  //Serial.begin(9600);   // инициализация порта для тестов
  btSerial.begin(9600);
  pinMode(LED, OUTPUT);
  pinMode(din, OUTPUT); //динамик
  digitalWrite(din, LOW);
  pinMode(sonarTrig, OUTPUT); //пин trig ультразвукового сонара
  digitalWrite(sonarTrig, LOW);
  pinMode(sonarEhcho, INPUT); //пин echo  ультразвукового сонара
//  motors_init;  //инициализация выходов моторов 
//  D11_Out;  D11_Low;    //динамик
  randomSeed(A6_Read); //Получить случайное значение 
  for(uint8_t i=0; i<5; i++) beep(70, random(100, 2000)); //звуковое оповещение готовности робота  
//  UART_Init(57600);//инициализация порта для связи с роутером  
  wdt_enable (WDTO_500MS);    //Сторожевая соба                                     ка 0,5сек.   
  pinMode(leftDirPin, OUTPUT);//инициализация выходов моторов
  pinMode(leftSpeedPin, OUTPUT);
  pinMode(rightDirPin, OUTPUT);
  pinMode(rightSpeedPin, OUTPUT);
}

void loop()
{ 
  if (btSerial.available())
  {
    char incomingByte = btSerial.read();
    switch (incomingByte)  //смотрим какая команда пришла
    {  
        case 'x':  //стоп
            digitalWrite(leftDirPin, LOW);
            digitalWrite(rightDirPin, LOW);
            analogWrite(leftSpeedPin, 0);
            analogWrite(rightSpeedPin, 0);
//        robot_stop;
        break; 
        
        case 'w':  //вперед
            digitalWrite(leftDirPin, HIGH);
            digitalWrite(rightDirPin, HIGH);
            analogWrite(leftSpeedPin, runSpeed1);
            analogWrite(rightSpeedPin, runSpeed1);
             //         robot_go;  
        break;  
        
        case 'a':  //лево
            digitalWrite(leftDirPin, HIGH);
            digitalWrite(rightDirPin, LOW);
            analogWrite(leftSpeedPin, runSpeed1);
            analogWrite(rightSpeedPin, runSpeed);
//          robot_rotation_left; 
        break;

        case 'd': //право
            digitalWrite(leftDirPin, LOW);
            digitalWrite(rightDirPin, HIGH);
            analogWrite(leftSpeedPin, runSpeed);
            analogWrite(rightSpeedPin, runSpeed1);
         //robot_rotation_right; 
        break; 
        
        case 's':  //назад
            digitalWrite(leftDirPin, LOW);
            digitalWrite(rightDirPin, LOW);
            analogWrite(leftSpeedPin, runSpeed);
            analogWrite(rightSpeedPin, runSpeed);
         //robot_back; 
        break;      
 
        case '1':  //светодиод
          digitalWrite(LED,HIGH);
         //robot_back; 
        break; 
        
        case '0':  //светодиод
          digitalWrite(LED,LOW);
         //robot_back; 
        break;      
   }  
    btSerial.print("*"); 
    btSerial.print(incomingByte); 
    btSerial.print("#"); 
    //Serial.println(incomingByte);
  }
  uint16_t dist=GetDistance(); //производим замер дистанции
  btSerial.print("*"); 
  btSerial.print(dist); 
  btSerial.print("#"); 
  wdt_reset(); //покормить собаку 
}
//***************************************************
uint16_t GetDistance() 
{
 uint16_t dist;
 uint8_t maxcomp=0; //счётчик максимального колличества совпадений
 uint16_t mcn;	//максимально часто встречающийся элемент массива
 uint16_t comp;	//временная переменная
 for (uint8_t i = 0; i < size_buff; ++i) //производим несколько замеров
  { 
   digitalWrite(sonarTrig, HIGH); //запустить измерение
   delayMicroseconds(10);
   digitalWrite(sonarTrig, LOW);  
   dist = pulseIn(sonarEhcho, HIGH, 2400); //считываем длительность времени прохождения эха, ограничить время ожидания
   if(dist==0) dist=2400;  
   sensor[i]=dist;  //сохранить в массиве
   delay(40); //задержка между посылками
   //wdt_reset(); //покормить собаку, что бы она не сбежала 
  }
  //dist=(find_similar(sensor, size_buff, 58))/58; // //фильтруем показания датчика и переводим в см
 	for (uint8_t i=0; i<size_buff; i++) 
	{
		comp=sensor[i];	//кладем элемент массива в comp
		uint8_t n=0;	//счётчик совпадении
		for (uint8_t j=0; j<size_buff; j++)	{ if (sensor[j]>comp-58 && sensor[j]<comp+58) n++;} // ищем повторения элемента comp в массиве buf	
		if (n > maxcomp) //если число повторов больше чем было найдено ранее
		{
			maxcomp=n; //сохраняем счетяик повторов
			mcn=comp; //сохраняем повторяемый элемент
		}		
	}
 return mcn/58;
}
//***************************************************
